#include "ros/ros.h"
#include <std_msgs/Int32.h>
#include <std_msgs/Float32.h>
#include "robo/Encoder.h"
#include "robo/Motor.h"
#include "robo/serial_adc_val.h"
#include "robo/Gyro.h"
#include <string.h>
#include "Serializer.cc"
#include <pthread.h>

using namespace robo;
Serializer * s;
ros::Subscriber sub1;
ros::Subscriber sub2;
ros::Subscriber sub3;
ros::Subscriber sub4;
ros::Subscriber sub5;
ros::Subscriber sub6;
ros::Subscriber sub7;

ros::Publisher  enc_pub;
ros::Publisher  adc_pub;

int retry_timer			= 1000;
int encoder_interval	= 500;
int adc_interval 		= 500;
bool read_adc0			= false;
bool read_adc1			= false;
bool read_adc2			= false;
bool read_adc3			= false;
bool read_adc4			= false;

void change_encoder_interval(const std_msgs::Int32::ConstPtr &msg)
{
	encoder_interval = msg->data;
	printf("encoder_interval: %i\n",msg->data);
}

void change_adc_interval(const std_msgs::Int32::ConstPtr &msg)
{
	adc_interval = msg->data;
	printf("adc_interval: %i\n",msg->data);
}

void change_retry_timer(const std_msgs::Int32::ConstPtr &msg)
{
	retry_timer = msg->data;
	printf("retry_timer: %i\n",msg->data);
}

void move_gyro(const Gyro::ConstPtr &msg)
{
	int pin = msg->pin_id;
	int rotation = msg->rotation;
	std::vector< std::pair<int,int> > cmd;
	cmd.push_back(std::make_pair(pin,rotation));
	while(!s->setServoPos(cmd)){usleep(retry_timer);}
}

void adc_enable(const std_msgs::Int32::ConstPtr &msg)
{
	switch(int(msg->data)){
		case(0):{read_adc0 = true;break;}
		case(1):{read_adc1 = true;break;}
		case(2):{read_adc2 = true;break;}
		case(3):{read_adc3 = true;break;}
		case(4):{read_adc4 = true;break;}
	}
}

void adc_dissable(const std_msgs::Int32::ConstPtr &msg)
{
	switch(int(msg->data)){
		case(0):{read_adc0 = false;break;}
		case(1):{read_adc1 = false;break;}
		case(2):{read_adc2 = false;break;}
		case(3):{read_adc3 = false;break;}
		case(4):{read_adc4 = false;break;}
	}
}

void change_motor_pwm(const Motor::ConstPtr &msg)
{
	printf("change motor pwm: L: %f , R:%f\n",msg->left,msg->right);
	while(!s->setMotorPWMs(msg->left, msg->right)){usleep(retry_timer);}
}

void * enc_thread_func( void *ptr ){
	ros::Rate loop_rate(1000);
	struct timeval start, end;
	while(ros::ok()){
		printf("1:enc pub\n");
		gettimeofday(&start, NULL);
		long r = 0;
		long l = 0;
		//while(!s->getEncoders(r, l)){usleep(retry_timer);}
		
		Encoder msg;
		msg.right 	= r;
		msg.left 	= l;
		msg.timestamp = start.tv_sec+1000000*start.tv_usec;
		enc_pub.publish(msg);
		
		ros::spinOnce();
		gettimeofday(&end, NULL);
		while((end.tv_sec*1000000+end.tv_usec-(start.tv_sec*1000000+start.tv_usec)) < encoder_interval*1000){
			loop_rate.sleep();
			gettimeofday(&end, NULL);
		}
		ros::spinOnce();
	}
}

void * adc_thread_func( void *ptr ){
	ros::Rate loop_rate(1000);
	struct timeval start, end;
	while(ros::ok()){
		printf("2:adc pub\n");
		gettimeofday(&start, NULL);
		std::vector< std::pair<int,int> > query;
		if(read_adc0){
			std::pair<int,int> p;
			p.first = 0;
			p.second = -1;
			query.push_back(p);
		}
		if(read_adc1){
			std::pair<int,int> p;
			p.first = 1;
			p.second = -1;
			query.push_back(p);
		}
		if(read_adc2){
			std::pair<int,int> p;
			p.first = 2;
			p.second = -1;
			query.push_back(p);
		}
		if(read_adc3){
			std::pair<int,int> p;
			p.first = 3;
			p.second = -1;
			query.push_back(p);
		}
		if(read_adc4){
			std::pair<int,int> p;
			p.first = 4;
			p.second = -1;
			query.push_back(p);
		}
		if(query.size() > 0){
			while(!s->readADC(query)){usleep(retry_timer);}
			gettimeofday(&end, NULL);
			serial_adc_val msg;
			msg.timestamp = end.tv_sec*1000000+end.tv_usec;
			msg.val0 = -1;
			msg.val1 = -1;
			msg.val2 = -1;
			msg.val3 = -1;
			msg.val4 = -1;
			for(unsigned int i = 0; i < query.size(); i++)
			{
				int ind = query.at(i).first;
				float val = query.at(i).second;
				switch(ind){
					case(0):{msg.val0 = val;break;}
					case(1):{msg.val1 = val;break;}
					case(2):{msg.val2 = val;break;}
					case(3):{msg.val3 = val;break;}
					case(4):{msg.val4 = val;break;}
				}
			}
			adc_pub.publish(msg);
		}
		
		ros::spinOnce();
		gettimeofday(&end, NULL);
		while((end.tv_sec*1000000+end.tv_usec-(start.tv_sec*1000000+start.tv_usec)) < adc_interval*1000){
			loop_rate.sleep();
			gettimeofday(&end, NULL);
		}
		ros::spinOnce();
	}
}

int main(int argc, char **argv)
{
	s = new Serializer();
	//s->connect("/dev/ttyS2");
	
	ros::init(argc, argv, "serializerNode");
	ros::NodeHandle n;
	ros::spinOnce();
	ros::Rate loop_rate(1000);
	
	enc_pub = n.advertise<Encoder>("/serial/encoder"	, 100000);
	pthread_t encoder_thread;
	pthread_create( &encoder_thread, NULL, enc_thread_func, NULL);
	
	adc_pub = n.advertise<serial_adc_val>("/serial/adc", 100000);
	pthread_t adc_thread;
	pthread_create( &adc_thread, NULL, adc_thread_func, NULL);
	
	
	sub1	= n.subscribe("/serial/encoder/interval", 1, change_encoder_interval);
	sub2	= n.subscribe("/serial/motor_pwm"		, 1, change_motor_pwm);
	sub3	= n.subscribe("/serial/adc/interval"	, 1, change_adc_interval);
	sub4	= n.subscribe("/serial/adc/enable"		, 1, adc_enable);
	sub5	= n.subscribe("/serial/adc/dissable"	, 1, adc_dissable);
	sub6 	= n.subscribe("/serial/gyro"			, 1, move_gyro);
	sub7 	= n.subscribe("/serial/retry_timer"		, 1, change_retry_timer);
	
	
	//s->disconnect();
	delete s;
	ros::spin();
	return 0;
}
